#define clockpin 13 // CI
#define enablepin 10 // EI
#define latchpin 9 // LI
#define datapin 11 // DI
 
#define NumLEDs 7
int count = 0;
int val = 0;
float distanceScale = 1023.0 / 7;
float average1 = 0;
float middle1 = 500.0;
float average2 = 0;
float middle2 = 500.0;
float average3 = 0;
float middle3 = 500.0;
float average4 = 0;
float middle4 = 500.0;
float threshold = 2.5;


int LEDChannels[NumLEDs][3] = {0};
int SB_CommandMode;
int SB_RedCommand;
int SB_GreenCommand;
int SB_BlueCommand;
 
void setup() {
  //Serial.begin(9600);
  delay(1000);
  middle1 = findAverage(0, 0);
  middle2 = findAverage(1, 0);
  middle3 = findAverage(2, 0);
  middle4 = findAverage(3, 0);
  /*
  Serial.print("middle1 is ");
  Serial.print(middle1);  
  Serial.print("middle2 is ");
  Serial.print(middle2);  
  Serial.print("middle3 is ");
  Serial.print(middle3);  
  Serial.print("middle4 is ");
  Serial.println(middle4);  
  average1 = findAverage(0, middle1);
  average2 = findAverage(1, middle2);
  average3 = findAverage(2, middle3);
  average4 = findAverage(3, middle4);
  Serial.print("average1 from ");
  Serial.print(middle1);
  Serial.print(" is ");
  Serial.println(average1);
  Serial.print("average2 from ");
  Serial.print(middle2);
  Serial.print(" is ");
  Serial.println(average2);
  Serial.print("average3 from ");
  Serial.print(middle3);
  Serial.print(" is ");
  Serial.println(average3);
  Serial.print("average4 from ");
  Serial.print(middle4);
  Serial.print(" is ");
  Serial.println(average4);
  Serial.println("calibrated");
  */

   pinMode(datapin, OUTPUT);
   pinMode(latchpin, OUTPUT);
   pinMode(enablepin, OUTPUT);
   pinMode(clockpin, OUTPUT);
   SPCR = (1<<SPE)|(1<<MSTR)|(0<<SPR1)|(0<<SPR0);
   digitalWrite(latchpin, LOW);
   digitalWrite(enablepin, LOW);
}
 
void SB_SendPacket() {
 
    if (SB_CommandMode == B01) {
     SB_RedCommand = 120;
     SB_GreenCommand = 100;
     SB_BlueCommand = 100;
    }
 
    SPDR = SB_CommandMode << 6 | SB_BlueCommand>>4;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_BlueCommand<<4 | SB_RedCommand>>6;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_RedCommand << 2 | SB_GreenCommand>>8;
    while(!(SPSR & (1<<SPIF)));
    SPDR = SB_GreenCommand;
    while(!(SPSR & (1<<SPIF)));
 
}
 
void WriteLEDArray() {
 
    SB_CommandMode = B00; // Write to PWM control registers
    for (int h = 0;h<NumLEDs;h++) {
	  SB_RedCommand = LEDChannels[h][0];
	  SB_GreenCommand = LEDChannels[h][1];
	  SB_BlueCommand = LEDChannels[h][2];
	  SB_SendPacket();
    }
 
    delayMicroseconds(15);
    digitalWrite(latchpin,HIGH); // latch data into registers
    delayMicroseconds(15);
    digitalWrite(latchpin,LOW);
 
    SB_CommandMode = B01; // Write to current control registers
    for (int z = 0; z < NumLEDs; z++) SB_SendPacket();
    delayMicroseconds(15);
    digitalWrite(latchpin,HIGH); // latch data into registers
    delayMicroseconds(15);
    digitalWrite(latchpin,LOW);
 
}

void loop() {
  val -= 2;
  if (val < 0) {
    val = 0;
  }

  int value = calculateValue();
  if (value > val) {
      val = value;
  }

  for (int i = 0; i < NumLEDs; i++) {  
  int hi = val;
   hi -= i * distanceScale;
   if (hi < 0) {
     hi = 0;
   }
   LEDChannels[i][0] = 1023;
   LEDChannels[i][1] = hi;
   LEDChannels[i][2] = hi;
  }
  
  WriteLEDArray();
  delay(10);
}

int calculateValue() {
  int value1 = readMics(0, middle1);
  int value2 = readMics(1, middle2);
  int value3 = readMics(2, middle3);
  int value4 = readMics(3, middle4);

  int offset = (value1 - average1) + (value2 - average2) + (value3 - average3) + (value4 - average4);
  if (offset > 100) {
      return 0;
  } else {
    return 0;
  }
}


void zero() {
  for (int i = 0; i < NumLEDs; i++) { 
    LEDChannels[i][0] = 0;
    LEDChannels[i][1] = 0;
    LEDChannels[i][2] = 0;
  }
}

float findAverage(int pin, int middle) {
  //Serial.print("calculating...");
  int samples = 10000;
  float total = 0.0;
  for (int i = 0; i < samples; i++) {
    int value = readMics(pin, middle);
    total += value;
  }
  
  float average = total / samples;
  //Serial.println("done!");
  return average;
}

int readMics(int pin, int middle) {
  int value = middle - analogRead(pin);
  return abs(value);
}
